#!/usr/bin/env python3
from __future__ import print_function
import rospy
import subprocess
import sys
import os

def main():
    # 加载ROS环境变量
    env = os.environ.copy()
    
    # 启动第一个节点（linktrack）
    rospy.loginfo("=== 正在启动 linktrack 节点... ===")
    node1 = subprocess.Popen(
        [
            'rosrun', 
            'nlink_parser',
            'linktrack',
            '_port_name:=/dev/ttyCH343USB0',
            '_baud_rate:=921600'
        ],
        env=env,
        stdout=subprocess.PIPE,
        stderr=subprocess.PIPE,
        text=True
    )
    
    # 等待节点初始化
    rospy.sleep(2)
    
    node2 = None  # 初始化第二个节点变量
    
    try:
        while not rospy.is_shutdown():
            # 显示操作提示
            print("\n" + "="*30)
            print("请选择操作：")
            print("1 - 建立通信连接（启动第二个节点）")
            print("2 - 断开通信连接（关闭第二个节点）")
            print("其他键 - 退出程序")
            print("="*30)
            
            # 获取用户输入
            if sys.version_info[0] == 2:
                user_input = raw_input().strip()
            else:
                user_input = input().strip()
            
            # 处理用户输入
            if user_input == '1':
                if node2 is None or node2.poll() is not None:
                    # 启动第二个节点
                    rospy.loginfo("=== 正在启动 sub_pose_twist 节点... ===")
                    node2 = subprocess.Popen(
                        [
                            'rosrun', 
                            'px4_uwb',
                            'sub_pose_twist'
                        ],
                        env=env,
                        stdout=subprocess.PIPE,
                        stderr=subprocess.PIPE,
                        text=True
                    )
                    rospy.sleep(2)
                    print("\n=== 通讯连接已经建立 ===")
                else:
                    print("\n=== 通讯连接已存在，无需重复建立 ===")
            
            elif user_input == '2':
                if node2 is not None and node2.poll() is None:
                    # 关闭第二个节点
                    rospy.loginfo("=== 正在关闭 sub_pose_twist 节点... ===")
                    node2.terminate()
                    node2.wait()
                    print("\n=== 通讯连接已断开 ===")  # 添加的断开提示
                else:
                    print("\n=== 没有活跃的通讯连接，无需断开 ===")
            
            else:
                # 退出程序
                print("\n=== 正在退出程序... ===")
                break
            
            # 检查第一个节点状态
            if node1.poll() is not None:
                rospy.logerr("linktrack 节点意外退出！")
                break
                
    except KeyboardInterrupt:
        rospy.loginfo("\n=== 检测到 Ctrl+C，正在关闭所有节点... ===")
    
    finally:
        # 确保所有节点都被关闭
        if node1.poll() is None:
            node1.terminate()
            node1.wait()
        if node2 is not None and node2.poll() is None:
            node2.terminate()
            node2.wait()
        rospy.loginfo("所有节点已关闭")

if __name__ == "__main__":
    try:
        rospy.init_node('node_starter', anonymous=True)
        main()
    except rospy.ROSInterruptException:
        rospy.loginfo("ROS节点被中断")
    except Exception as e:
        rospy.logerr("程序异常：%s" % str(e))
    